"""
@作者：sipeiliu@163.com

@节点身份：frame_image的话题发布者，camera_state动作的服务端

@节点任务：采集图像
"""

# ros2库
import rclpy # ros2 python接口库
from rclpy.node import Node # ros2 节点类 
from rclpy.action import ActionServer # 动作客户端库
from cv_bridge import CvBridge # ros2与OpenCv图象转换类

# 其它库
import cv2 # OpenCv图像处理库
import time # 时间库


# 导入消息类型
from sensor_msgs.msg import Image # 图像消息类型
from facer_interfaces.action import CameraState # 自定义摄像机状态消息类型

"""采集图像节点类"""
class CaptureImage(Node):

    """构造函数"""
    def __init__(self):
        super().__init__('capture_image') # ros2节点父类初始化,节点名称为capture_image
        
        '''frame_image话题的发布者的相关代码'''
        self.cap = cv2.VideoCapture(0)# 创建视频采集对象
        self.cv_bridge = CvBridge() # 创建图象转换对象，该对象可以将OpenCv采集到的图像转换为ros2的图像消息类型
        self.publisher_ = self.create_publisher(Image, 'frame_image', 10) # 创建话题发布者对象(消息类型、话题名、队列长度)
        
        # timer_period = 0.1 # 确定发布周期，单位为秒 
        self.declare_parameter('timer_period', 0.1) # 修改发布周期为参数 
        timer_period_param = self.get_parameter('timer_period').get_parameter_value().double_value  
        
        self.timer = self.create_timer(timer_period_param, self.timer_callback) # 创建定时器


        '''camera_state动作的服务端的相关代码'''
        self._action_server = ActionServer(self, CameraState, 'camera_state', self.execute_callback) # 创建动作服务端对象(消息类型、动作名、回调函数)


    '''发布话题定时函数'''
    def timer_callback(self):
        ret, frame = self.cap.read() # 读取一帧图像
        if ret ==  True: # 返回值为True则摄像头已打开，采集图像成功     
            self.camera_state = '摄像头打开'
            ros2_image = self.cv_bridge.cv2_to_imgmsg(frame, 'bgr8') # 将OpenCv采集到的图像转换为ros2的图像消息类型
            self.publisher_.publish(ros2_image) # 发布图像
        else:
            self.camera_state = '摄像头关闭'

    '''执行动作回调函数'''
    def execute_callback(self, goal_handle):
        feedback_count = 0 # 返回次数
        feedback_msg = CameraState.Feedback() # 实例化动作的反馈
        while goal_handle.request.start == True and feedback_count < 10 : # 动作请求为真且反馈次数为10
            feedback_msg.state = self.camera_state # 传递摄像头状态
            goal_handle.publish_feedback(feedback_msg) # 发送周期反馈
            feedback_count = feedback_count + 1 # 统计反馈次数
            time.sleep(0.5) # 隔0.5秒就反馈一次

        goal_handle.succeed() # 表明目标成功，否则会警告目标状态没有设置

        result = CameraState.Result() # 实例化动作的结果
        result.finish = '动作完成' # 给动作结果的finish赋值，动作完成
        return result # 返回动作结果
      
'''ros2 节点主入口main函数'''
def main(args=None):
    rclpy.init(args=args) # ros2 python接口初始化

    node = CaptureImage() # 创建ros2节点对象并进行初始化

    rclpy.spin(node) # 循环等待ros2退出

    node.destory_node() # 销毁节点对
    rclpy.shutdown() # 关闭ros2 python接口

# 使模块是顶层程序文件是能运行，是导入模块时不会运行
if __name__ == "__main__":
    main()